from bresham import draw_line_to_boundary



def assume_track_data(robot_pose, T, robot_speed):
    x0, y0, a0 = robot_pose # [50, 100, 0] 
    EVERY_GRID = .1
    X = [t * int(EVERY_GRID/robot_speed) + x0  for t in range(T)]
    Y = [y0] * T
    A = [0] * T

    return X,Y,A


def gen_sensor_data(real_map, robot_pose, robot_speed):
    # 运动时间 t 和机器的初始位置

    T = 100
    x0, y0, a0 = robot_pose # [0, 100, 0] 

    # 轨迹方程(关于T)
    X, Y, A = assume_track_data(robot_pose, T, robot_speed)

    LEFT = []
    RIGHT = []

    for i in range(T):
        # 计算障碍物的点在机器坐标下
        # x_now = int(X[i] * 10) 
        # y_now = int(Y[i] * 10)
        x_now = X[i]
        y_now = Y[i]
        find_points_left  = draw_line_to_boundary(x_now, y_now, -45, real_map.shape[0], real_map.shape[1])
        find_points_right = draw_line_to_boundary(x_now, y_now, 45, real_map.shape[0], real_map.shape[1])

        pass
        for left in find_points_left:
            x,y = left
            x,y = int(x), int(y)
            if real_map[x][y] == 1:
                LEFT.append((x,y))
            
        for right in find_points_right:
            x,y = right
            x,y = int(x), int(y)
            if real_map[x][y] == 1:
                RIGHT.append((x,y))

    sensor_data_frames = [(a, b) for a, b in zip(LEFT, RIGHT)]
    robot_pose_frames = [(a,b,c) for a,b,c in zip(X,Y,A)]
    return  sensor_data_frames, robot_pose_frames

    # return sensor_data_frames, sensor_angles, robot_pose_frames